/*
	Matrix.cpp

		Matrix manipulation.

	Author:	Brett Porter
	Email: brettporter@yahoo.com
	Website: http://rsn.gamedev.net/pl3d
	Copyright (C)2000, 2001, Brett Porter. All Rights Reserved.
	This source code is released under the LGPL. See license.txt for details.

	Created: 9 December 2000
	Last Edited: 8 January 2001

	Please see the file ChangeLog.html for a revision history.
*/
#include "FoGGEDEF.h"
#include "foggeCommonMath.h"

namespace _FOGGE_
{
	namespace _UTIL_
	{

		namespace _MATH_
		{
			Matrix::Matrix()
			{
				loadIdentity();
			}

			Matrix::~Matrix()
			{
			}

			void Matrix::postMultiply( const Matrix& matrix )
			{
				float newMatrix[16];
				const float *m1 = m_matrix, *m2 = matrix.m_matrix;

				newMatrix[0] = m1[0]*m2[0] + m1[4]*m2[1] + m1[8]*m2[2];
				newMatrix[1] = m1[1]*m2[0] + m1[5]*m2[1] + m1[9]*m2[2];
				newMatrix[2] = m1[2]*m2[0] + m1[6]*m2[1] + m1[10]*m2[2];
				newMatrix[3] = 0;

				newMatrix[4] = m1[0]*m2[4] + m1[4]*m2[5] + m1[8]*m2[6];
				newMatrix[5] = m1[1]*m2[4] + m1[5]*m2[5] + m1[9]*m2[6];
				newMatrix[6] = m1[2]*m2[4] + m1[6]*m2[5] + m1[10]*m2[6];
				newMatrix[7] = 0;

				newMatrix[8] = m1[0]*m2[8] + m1[4]*m2[9] + m1[8]*m2[10];
				newMatrix[9] = m1[1]*m2[8] + m1[5]*m2[9] + m1[9]*m2[10];
				newMatrix[10] = m1[2]*m2[8] + m1[6]*m2[9] + m1[10]*m2[10];
				newMatrix[11] = 0;

				newMatrix[12] = m1[0]*m2[12] + m1[4]*m2[13] + m1[8]*m2[14] + m1[12];
				newMatrix[13] = m1[1]*m2[12] + m1[5]*m2[13] + m1[9]*m2[14] + m1[13];
				newMatrix[14] = m1[2]*m2[12] + m1[6]*m2[13] + m1[10]*m2[14] + m1[14];
				newMatrix[15] = 1;

				set( newMatrix );
			}

			void Matrix::setTranslation( const Vector* translation )
			{
				m_matrix[12] = translation->x;
				m_matrix[13] = translation->y;
				m_matrix[14] = translation->z;
			}

			void Matrix::setInverseTranslation( const Vector* translation )
			{
				m_matrix[12] = -translation->x;
				m_matrix[13] = -translation->y;
				m_matrix[14] = -translation->z;
			}

			
			void Matrix::setRotation( const Rotator& rot )
			{//yaw pitch roll
				double cr = cos( rot.m_fRoll * PI/180.0 );
				double sr = sin( rot.m_fRoll * PI/180.0 );
				double cp = cos( rot.m_fPitch * PI/180.0 );
				double sp = sin( rot.m_fPitch * PI/180.0 );
				double cy = cos( rot.m_fYaw * PI/180.0  );
				double sy = sin( rot.m_fYaw * PI/180.0 );

				double sysr = sy*sr;
				double cysr = cy*sr;
				double cycr = cy*cr;
				double sycr = sy*cr;

				m_matrix[0] = ( float )( cy*cp);
				m_matrix[1] = ( float )( cycr*sp + sysr);
				m_matrix[2] = ( float )( cysr*sp - sycr );

				
				m_matrix[4] = ( float )( -sp );
				m_matrix[5] = ( float )( cp*cr );
				m_matrix[6] = ( float )( cp*sr );

				m_matrix[8] = ( float )( sy*cp );
				m_matrix[9] = ( float )( sycr*sp - cysr );
				m_matrix[10] = ( float )( sysr*sp + cycr );
			}

			void Matrix::setInverseRotation( const Rotator& rot )
			{//yaw pitch roll
				double cr = cos( rot.m_fRoll * PI/180.0 );
				double sr = sin( rot.m_fRoll * PI/180.0 );
				double cp = cos( rot.m_fPitch * PI/180.0 );
				double sp = sin( rot.m_fPitch * PI/180.0 );
				double cy = cos( rot.m_fYaw * PI/180.0  );
				double sy = sin( rot.m_fYaw * PI/180.0 );

				double sysr = sy*sr;
				double cysr = cy*sr;
				double cycr = cy*cr;
				double sycr = sy*cr;

				m_matrix[0] = ( float )( cy*cp);
				m_matrix[4] = ( float )( cycr*sp + sysr);
				m_matrix[8] = ( float )( cysr*sp - sycr );

				
				m_matrix[1] = ( float )( -sp );
				m_matrix[5] = ( float )( cp*cr );
				m_matrix[9] = ( float )( cp*sr );

				m_matrix[2] = ( float )( sy*cp );
				m_matrix[6] = ( float )( sycr*sp - cysr );
				m_matrix[10] = ( float )( sysr*sp + cycr );
			}

			void Matrix::setRotationQuaternion( const Quaternion& quat )
			{
				m_matrix[0] = ( float )( 1.0 - 2.0*quat[1]*quat[1] - 2.0*quat[2]*quat[2] );
				m_matrix[1] = ( float )( 2.0*quat[0]*quat[1] + 2.0*quat[3]*quat[2] );
				m_matrix[2] = ( float )( 2.0*quat[0]*quat[2] - 2.0*quat[3]*quat[1] );

				m_matrix[4] = ( float )( 2.0*quat[0]*quat[1] - 2.0*quat[3]*quat[2] );
				m_matrix[5] = ( float )( 1.0 - 2.0*quat[0]*quat[0] - 2.0*quat[2]*quat[2] );
				m_matrix[6] = ( float )( 2.0*quat[1]*quat[2] + 2.0*quat[3]*quat[0] );

				m_matrix[8] = ( float )( 2.0*quat[0]*quat[2] + 2.0*quat[3]*quat[1] );
				m_matrix[9] = ( float )( 2.0*quat[1]*quat[2] - 2.0*quat[3]*quat[0] );
				m_matrix[10] = ( float )( 1.0 - 2.0*quat[0]*quat[0] - 2.0*quat[1]*quat[1] );
			}

			void Matrix::inverseRotateVect( Vector* vect) const
			{
				
				vect->x = vect->x*m_matrix[0]+vect->y*m_matrix[1]+vect->z*m_matrix[2];
				vect->y = vect->x*m_matrix[4]+vect->y*m_matrix[5]+vect->z*m_matrix[6];
				vect->z = vect->x*m_matrix[8]+vect->y*m_matrix[9]+vect->z*m_matrix[10];

				
			}

			void Matrix::inverseTranslateVect( Vector* vect) const
			{
				vect->x = vect->x-m_matrix[12];
				vect->y = vect->y-m_matrix[13];
				vect->z = vect->z-m_matrix[14];
			}

		}
	}
}